/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "JointGroupCtrlClient.hpp"
#include "TextClient.hpp"
#include "ImageClient.hpp"
#include "PositionClient.hpp"
#include "RangingClient.hpp"
#include "SpeedCtrlClient.hpp"
#include "CoordinateDriveClient.hpp"
#include "MapClient.hpp"


void showUsage();


static gimi::GIMI g;


static MaCI::JointGroupCtrl::CJointGroupCtrlClient ptu(&g, 0); /// Client for PTU
static MaCI::Text::CTextClient tts(&g, 0); /// Client for TTS
static MaCI::Image::CImageClient image(&g, 0); /// Image Client
static MaCI::Position::CPositionClient positionClient(&g,0); ///Position Client
static MaCI::Ranging::CRangingClient rangingClient(&g,0);///Ranging Client (Laser)
static MaCI::SpeedCtrl::CSpeedCtrlClient speedCtrlClient(&g,0);///Speed Control Client
static MaCI::CoordinateDrive::CCoordinateDriveClient coordinateDriveClient(&g,0);
static MaCI::Map::CMapClient mapClient(&g, 0);

bool run =true;
int verbose = 1;


/**
 * Wrapper for send speed command (mainly for preventing sending commands, 
 * when in emergencyMode)
 * @param speed forward velocity
 * @param angularSpeed Angular velocity
 */
void sendSpeedCommand(float speed, float angularSpeed){
		float acceleration = 0.2;
		speedCtrlClient.SetSpeed(speed, angularSpeed, acceleration);
}

void sendCoordinateCommand(float x, float y){
		coordinateDriveClient.RemoveCoordinates(); ///< Remove coordinates
		coordinateDriveClient.SetCoordinate(	x,y,0,false, true, false); ///< send new target
}


///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// MAIN Application Runs here 
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
void Execute(){
		MaCI::Ranging::CRangingData rd;
		int rangingSeq = -1;
		bool rr,pr;
		MaCI::Position::CPositionData pd_timed;
		gim::time measure_diff;
		///////////////////////////////////////////////////////////////////////////////////
		/// Get Map From the Map Server
		///////////////////////////////////////////////////////////////////////////////////
		MaCI::Map::CMapData MapData;
		dPrint(1,"Getting the whole current map from server...");
		if (mapClient.GetMap(MapData,4000)){
				dPrint(1,"Got the whole map");
				MapData.Print(1);
				dPrint(1,"Contents:");
				for (int i=0;i<MapData.GetMapLineCount();i++){
						const MaCI::Map::TMapLine * l=MapData.GetMapLine(i);
						dPrint(1,"Line: x1=%f y1=%f x2=%f y2=%f",l->x1, l->y1, l->x2, l->y2);
				}
		}
		
		
		while(run){
				///////////////////////////////////////////////////////////////////////////////////
				/// Get Laser Scan
				///////////////////////////////////////////////////////////////////////////////////
				if(!(rr = rangingClient.GetDistanceArray(rd, &rangingSeq, 1000))){
						dPrint(1,"Failed to get DistanceArray?");
				}else{
						dPrint(1,"Received %d range measurements",rd.GetDistanceElementCount());
						const MaCI::Ranging::TDistance *r_meas;
						for(int i=0;i<rd.GetDistanceElementCount();i++){
								r_meas = rd.GetDistance(i);
								dPrint(6,"r[%d]=%.2f a[%d]=%.2f",i,r_meas->distance,i,r_meas->angle);
						}
				}
				///////////////////////////////////////////////////////////////////////////////////
				/// Get Pose (timestamp as close as possible to the range scan)
				///////////////////////////////////////////////////////////////////////////////////
				const MaCI::Common::TTimestamp *ts = rd.GetTimestamp(); // Timestamp from distancearray
				if(ts){
					pr = positionClient.GetPositionEvent(pd_timed, ts->GetGimTime(), &measure_diff);
					if (!pr) {
							dPrint(1,"Failed to get PositionEvent (timed)?");
					} else {
							const MaCI::Position::TPose2D *odo; ///< Odometry from the robot
							odo = pd_timed.GetPose2D();  ///< pointer to odometry data
							
							dPrint(6,"Got Pose (%.2f, %.2f, %.2f) time difference: %s",odo->x,odo->y,odo->a,
																																				measure_diff.toString().c_str());
					}
				}
				MaCI::CoordinateDrive::TCoordinate2D coordHolder;
                                MaCI::Common::TTimestamp latestPointTimestamp;
				if (coordinateDriveClient.GetLatestPointReached(coordHolder, latestPointTimestamp,NULL, 0)){
						dPrint(1,"The robot has reached to Target (%f, %f)", coordHolder.x, coordHolder.y);
				}
				ownSleep_ms(50);
		}
}

///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
/// MAIN
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
int main(int argc, char *argv[]) {
  // GIMnet parameters
		std::string gimnetAP = "rousku.dyndns.tv";
		int gimnetAPPort = 40002;
		std::string gimnetName = "";
		std::string maciGroup="J2B2";
	
		std::string PTU_Instance_Name="ServoCtrl";
		std::string TTS_Instance_Name="TTS";
		std::string Image_Instance_Name="CameraFront";
		std::string Position_Instance_Name = "Motion";
		std::string Ranging_Instance_Name = "Laser";
		std::string SpeedCtrl_Instance_Name = "Motion";
		std::string CoordinateDrive_Instance_Name = "PLANNER";
		std::string Map_MaCI_Source = "AutLab";
		std::string Map_Instance_Name = "autlab-small";
		
						
  // Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(3);
		debugSetLogFilename("IO_Client_example.log");
		debugSetLogLevel(10);
  // Greet
		fprintf(stderr,
						"\nIOClientExample - MaCI IO Interface Example - v0.0\n");

  // Parse parameters
		bool parse = true;
		while(parse) {
				int c;
				c = getopt(argc, argv, "s:n:u:p:g:S:T:C:hqv");
				switch(c) {

						/////////////////////////////////
						////// Application specific /////
						/////////////////////////////////

						/////////////////////////////////
						///// GIMnet parameters /////////
						/////////////////////////////////
						case 'n':
								gimnetName = optarg;
								dPrint(3,"Using GIMnet Name '%s'", optarg);
								break;

						case 'u':
								gimnetAP = optarg;
								dPrint(3,"Using GIMnetAP address '%s'", optarg);
								break;

						case 'p':
								gimnetAPPort = atoi(optarg);
								dPrint(3,"Using GIMnetAP port '%d'", gimnetAPPort);
								break;
						case 'g':
						case 'G':
								maciGroup = optarg;
								dPrint(3,"Using MaCI Group: '%s'", optarg);
								break;
						case 'S':
								PTU_Instance_Name = optarg;
								dPrint(3,"PTU target is: '%s'", optarg);
								break;
						case 'T':
								TTS_Instance_Name = optarg;
								dPrint(3,"Text Target is: '%s'", optarg);
								break;
						case 'C':
								Image_Instance_Name = optarg;
								dPrint(3,"Camera Image Target is: '%s'", optarg);
								break;
								//////////////////////////////////
								//// GENERIC:                 ////
								//////////////////////////////////
						case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
								if (verbose > 0)
										debugSetGlobalDebugLvl(++verbose);
								break;
      
						case 'q':
								verbose = 0;
								debugSetGlobalDebugLvl(0);
								break;
      
						case 'h':
						case '?':
								showUsage();
								exit(1);
								break;
      
						case -1:
								parse = false;
								break;
				}
		}


		MaCI::EMaCIError e;
  
		int r;
		if ( (r = g.connectToHubEx(gimnetAP, gimnetAPPort, gimnetName) ) != GIMI_OK) {
				dPrint(1,"Failed to connect to GIMnet AccessPoint '%s:%d' with name '%s': '%s'",
							 gimnetAP.c_str(), 
							 gimnetAPPort, 
							 gimnetName.size()?gimnetName.c_str():"<anonymous>",
							 gimi::getGimiErrorString(r).c_str());
    
		} else {
				dPrint(2,"Succesfully connected to GIMnet at AccessPoint '%s:%d' with name '%s': '%s'",
							 gimnetAP.c_str(), 
							 gimnetAPPort, 
							 gimnetName.size()?gimnetName.c_str():"<anonymous>",
							 gimi::getGimiErrorString(r).c_str());
		
				/////////////////////////////////////////////////////////////////////////////
				/////////////////////////////////////////////////////////////////////////////
		/// GET ALL MACI SERVICES FROM GIMNET
				/////////////////////////////////////////////////////////////////////////////
				/////////////////////////////////////////////////////////////////////////////
		// MaCICtrl client
				MaCI::MaCICtrl::CMaCICtrlClient ctrlclient(&g); ///< Open MaCICtrl client.
		
				MaCI::MaCICtrl::TServiceEntryArray services; ///< the services
		
				if (ctrlclient.Open() == MaCI::KMaCIOK) {
    // Get all MaCI services from local GIMnet.
						gimi::t_serviceList macictrlservicelist;
						if (ctrlclient.DiscoverMaCICtrlServices(macictrlservicelist,1000) == MaCI::KMaCIOK) {	
				// Request information from all MaCI nodes on the Net.			
								if (ctrlclient.GetServiceArray(services, macictrlservicelist,1000) != MaCI::KMaCIOK) {
										dPrint(1,"Failed to get service array - aborting");
										exit(1);
								}
						}else{
								dPrint(1,"Failed to receive MaCI services from the GIMnet - Aborting - We are Wery sorry!");
								exit(1);
						}
				}
		
				ptu.SetDefaultTarget(
							MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_JointGroupCtrl."+ PTU_Instance_Name),services);
		
				tts.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_Text."+ TTS_Instance_Name),services);
				
				image.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_Image."+ Image_Instance_Name),services);
				
				positionClient.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_Position."+ Position_Instance_Name),services);
				
				rangingClient.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_Ranging."+ Ranging_Instance_Name),services);
				
				speedCtrlClient.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_SpeedCtrl."+ SpeedCtrl_Instance_Name),services);
				
				coordinateDriveClient.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(maciGroup + ".MaCI_CoordinateDrive."
																						+CoordinateDrive_Instance_Name),services);
				mapClient.SetDefaultTarget(
								MaCI::MaCICtrl::SMaCISL(Map_MaCI_Source + ".MaCI_Map."+Map_Instance_Name),services);
				
				if(ptu.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open PTU");
				}
				if(tts.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open TTS");
				}
				if(image.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open Image");
				}
				if(positionClient.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open Position");
				}
				if(rangingClient.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open Laser");
				}
				if(speedCtrlClient.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open speedCtrlClient");
				}
				if(coordinateDriveClient.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open Coordinate drive");
				}
				if(mapClient.Open() != MaCI::KMaCIOK){
						dPrint(1,"Failed to open MapClient");
				}
				
				///FIXME:: Executes now even if not all the services are initialized
				Execute();
				ptu.Close();
				tts.Close();
				image.Close();
				positionClient.Close();
				rangingClient.Close();
				speedCtrlClient.Close();
				coordinateDriveClient.Close();
				mapClient.Close();
				
		}
  
		return 0;
}


void showUsage()
{
		fprintf(stderr,"\n");
		fprintf(stderr,"Usage:\t3MUI [OPTIONS]\n");
		fprintf(stderr,"  [-u addr]\t\t\tUse 'addr' as GIMnet AccessPoints address\n");
		fprintf(stderr,"  [-p port]\t\t\tUse 'port' as GIMnet AccessPoints port\n");
		fprintf(stderr,"  [-n name]\t\t\tUse 'name' as this nodes GIMnet name\n");
		fprintf(stderr,"  [-S name]\t\t\tUse 'name' as PTU service instance name\n");
		fprintf(stderr,"  [-T name]\t\t\tUse 'name' as TTS service instance name\n");
		fprintf(stderr,"  [-C name]\t\t\tUse 'name' as Camera Image service instance name\n");
		
		fprintf(stderr,"  [-v]\t\t\t\tIncrease verbosity by 1\n");
		fprintf(stderr,"  [-q]\t\t\t\tQuiet mode\n");
		fprintf(stderr, "\n\n");
}
//*****************************************************************************
